#include "../User/Component/PID.h"

#include <stdint.h>

float PID_Apply(PID_Param_t *pid, float feedback, float differential,
                float setpoint) {
  /* 计算误差 */
  float angle_gap = setpoint - feedback;
  /* 积分 */
  pid->advantage.integral += angle_gap;
  /* 积分限幅 */
  if (pid->advantage.integral > pid->advantage.i_limit)
    pid->advantage.integral = pid->advantage.i_limit;
  if (pid->advantage.integral < -pid->advantage.i_limit)
    pid->advantage.integral = -pid->advantage.i_limit;
  /* 计算微分 */
  float out = differential * pid->d;
  /* 微分限幅 */
  if (out > pid->advantage.d_out_limit) out = pid->advantage.d_out_limit;
  if (out < -pid->advantage.d_out_limit) out = -pid->advantage.d_out_limit;
  /* 计算输出 */
  out += angle_gap * pid->p + pid->advantage.integral * pid->i;
  /* 输出限幅 */
  if (out > pid->advantage.out_limit) out = pid->advantage.out_limit;
  if (out < -pid->advantage.out_limit) out = -pid->advantage.out_limit;

  return out * pid->advantage.k;
}
